/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/GeometrySrc/transfor.cpp
 * @Description  : 实现了Transform类，该类用于在局部坐标系和世界坐标系之间进行转换。 
 *                 包括从局部坐标系到世界坐标系的点转换，以及姿态的转换。 
 *                 提供了初始化局部坐标系原点和倾斜角度的方法。 
 *                 还包括从世界坐标系到局部坐标系的逆转换。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:44:20
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "geometry.h" 
//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CTransform".
//
//   CTransform: The constructor.
//
Transform::Transform(LocalFrame& local_frame)
{
    origin_ = local_frame.origin_;
    slant_ = local_frame.slant_;
}

//
//   CTransform: Overloaded constructor.
//
Transform::Transform(const Point& origin, const Angle& slant) 
{
    origin_ = origin;
    slant_ = slant;
}

//
//   Init the origin and slant angle of the local frame.
//   pstLocal is global map pos
void Transform::Init(Posture& local_posture) 
{
    origin_ = local_posture.GetPntObject();
    slant_ = local_posture.GetAngle();
}

//
//   Convert points in the local coordinate system to the world coordinate system
//   GetWorldPoint: A transformation from the local frame to the
//   world frame.
//   If the two coordinate systems x and y are parallel
//   m_angSlant = 0.0 It's a special case so sin(m_angSlant) = 0  cos(m_angSlant)=1
Point& Transform::GetWorldPoint(const Point& local_point) 
{
    float dx = local_point.x_ * cos(slant_.radian_) - local_point.y_ * sin(slant_.radian_);
    float dy = local_point.y_ * cos(slant_.radian_) + local_point.x_ * sin(slant_.radian_);
    point_.x_ = origin_.x_ + dx;
    point_.y_ = origin_.y_ + dy;
    return point_;
}

//
//   Convert points in the world coordinate system to the local coordinate system
//   GetWorldPoint: A reverse transformation from the world frame to the
//   local frame.
//
Point& Transform::GetLocalPoint(const Point& world_point) 
{
    float x0 = world_point.x_ - origin_.x_;
    float y0 = world_point.y_ - origin_.y_;
    point_.x_ = x0 * cos(slant_.radian_) + y0 * sin(slant_.radian_);
    point_.y_ = y0 * cos(slant_.radian_) - x0 * sin(slant_.radian_);
    return point_;
}

//
//   Position and pose of local points in the world
//   GetWorldPosture: A posture transformation from the local frame to
//   the world frame.
//
Posture& Transform::GetWorldPosture(Posture& local_posture) 
{
    Angle ang = slant_ + local_posture.theta_;
    posture_.SetPnt(GetWorldPoint(local_posture.GetPntObject()));
    posture_.SetAngle(ang);
    return posture_;
}

//
//   The position and pose of the world point in the local
//   GetLocalPosture: A reverse posture transformation from the world
//   frame to the local frame.
//
Posture& Transform::GetLocalPosture(Posture& world_posture)
{
    Angle ang = world_posture.GetAngle() - slant_;
    posture_.SetPnt(GetLocalPoint(world_posture.GetPntObject()));
    posture_.SetAngle(ang);
    return posture_;
}
